#pragma once
#include "base_interface.h"
#include <rclcpp/rclcpp.hpp>
#include <memory>

namespace io_module {

class ModbusInterface : public BaseInterface {
public:
    ModbusInterface(
        std::shared_ptr<rclcpp::Node>& node,
        const ModuleParam& param
    );

    virtual ~ModbusInterface();

    bool init() override;
    bool setOutput(const uint8_t &index, const bool &level) override;
    bool readInput(uint64_t &result) override;
    bool setOutputA(const uint8_t &index, const WORD &rawData) override;
    bool readInputA(std::vector<WORD> &result) override;

private:
    static rclcpp::QoS get_qos_profile() {
        return rclcpp::QoS(rclcpp::KeepLast(5))
            .reliable()
            .durability_volatile();
    }

    void setup_ros_communications();
    void digital_callback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr msg);
    void analog_callback(const std_msgs::msg::UInt16MultiArray::ConstSharedPtr msg);

    rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr digital_out_pub_;
    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr digital_in_sub_;
    
    rclcpp::Publisher<std_msgs::msg::UInt16MultiArray>::SharedPtr analog_out_pub_;
    rclcpp::Subscription<std_msgs::msg::UInt16MultiArray>::SharedPtr analog_in_sub_;

    std::mutex digital_mutex_;
    std::mutex analog_mutex_;
};

} // namespace io_module